#include "i2c-dev.h"
#include <stdio.h>
unsigned char model1 = 0x0;
unsigned char model2 = 0x1;
unsigned char defaultAddr = 64; //0b1000000
unsigned char pca9685PreScale = 0xfe;
unsigned int nowfreq = 25000000;
int fd = NULL;
//Full = 0x10 # bit4 used to set full On/Off
#define PWMonregL(x) (6 +4 * x)
#define PWMonregH(x) (7 +4 * x)
#define PWMoffregL(x) (8 +4 * x)
#define PWMoffregH(x) (9 +4 * x)

#define WFLY_LOW(x)  pwm_setPWMSignal(x, x*450, 194 + 450 * x)
#define WFLY_MID(x) pwm_setPWMSignal(x, x*450, 290 + 450 * x)
#define WFLY_HIGH(x) pwm_setPWMSignal(x, x*450, 387 + 450 * x)

#define WFLY_DIRLF 0 // 左右
#define WFLY_DIRUD 2 // 上下
#define WFLY_POWER 1 // 油门
#define WFLY_SPIN 3  // 自旋转
unsigned char pwm_reset()
{
	unsigned char r;
	r = I2CWriteByteTo(fd,  model1, 0, 1);
	// r = r & pwm_writereg(model2, 0x00);
	return r;
}

unsigned char pwm_setFrequency(unsigned char prescalevalue)
{
	unsigned char r;
	// unsigned char prescalevalue = 138;//(25000000/(4096*freq))-1;
	unsigned char oldmode, newmode;
	printf("prescalevalue: %d\n",(int)prescalevalue);
	oldmode = I2CReadByteFrom(fd, model1, 1);
	newmode = (oldmode & 0x7f) | 0x10;
	r = I2CWriteByteTo(fd, model1, newmode, 1);
	r = r | I2CWriteByteTo(fd, pca9685PreScale, prescalevalue, 1);
	r = r | I2CWriteByteTo(fd, model1, oldmode, 1);
	usleep(5*1000);
	oldmode = oldmode | 0x80;
	//set restart bit (causes remembering where was when off)
	r = r | I2CWriteByteTo(fd, model1, oldmode, 1);
	return r;
}

unsigned char pwm_setPWMSignal(unsigned char lednum, unsigned short onTime, unsigned short offTime)
{
	unsigned char r = 0;
	unsigned char onTimeL, onTimeH, offTimeL, offTimeH;
	// unsigned int onTime, offTime;
	// calculate on off Time
	// onTime = (phaseShift * 4096) / nowfreq;
	// offTime = (dutyCycle * 4096) / nowfreq + onTime;
	onTimeL = onTime & 0xff;
	onTimeH = onTime >> 8;
	offTimeL = offTime & 0xff;
	offTimeH = offTime >> 8;

	r |= I2CWriteByteTo(fd,  PWMonregL(lednum), onTimeL, 1);
	r |= I2CWriteByteTo(fd, PWMonregH(lednum), onTimeH, 1);
	r |= I2CWriteByteTo(fd, PWMoffregL(lednum), offTimeL, 1);
	r |= I2CWriteByteTo(fd, PWMoffregH(lednum), offTimeH, 1);
	return r;
}

unsigned char wfly_init()
{
	pwm_reset();
	unsigned char r;
	r = pwm_setFrequency(138); // 50Hz

	unsigned short ont = 0,offt = 290;
	unsigned char i = 0;
	for(i = 0;i < 4;i ++)
	{

		r = r | pwm_setPWMSignal(i, ont, offt);
		ont += 450;
		offt += 450;
	}
	return r;
}

unsigned char wfly_lauch()
{
	unsigned char r = 0;
	r |= WFLY_HIGH(0);//WFLY_DIRUD);
	r |= WFLY_LOW(1);//WFLY_POWER);
	r |= WFLY_LOW(2);//WFLY_SPIN);
	r |= WFLY_HIGH(3);//WFLY_DIRLF);
	return r;
}

unsigned char wfly_stop()
{
	unsigned char r = 0;
	r = WFLY_MID(WFLY_DIRUD);
	r = r | WFLY_LOW(WFLY_POWER);
	r = r | WFLY_MID(WFLY_SPIN);
	r = r | WFLY_MID(WFLY_DIRLF);
	return r;
}

void InitFunc()
{
	fd = open("/dev/i2c-0", O_RDWR);
	if(!fd)
	{
		printf("open /dev/i2c-0 faild\n");
		return;
	}
	setI2CSlave(fd, defaultAddr);
	return;
}

void ExitFunc()
{
	if(fd)
		close(fd);
}

int main()
{
	// note : when WFLY_DIRUD key was down, the signel was high
	unsigned char r=64;
	//unsigned char va=123;
	InitFunc();
	pwm_reset();
	printf("init wfly...\n");
	r = wfly_init();

	printf("launch after 3 second... \n");
	sleep(3);
	printf("launch!!! \n");
	r = r |wfly_lauch();
	sleep(3);
	printf("stop\n");
	r = r | wfly_stop();
	//int i;
	if(r==0)
		printf("send data success!\n");
	else
		printf("send data fail\n");
	ExitFunc();
	return r;
}
